#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "tf2_ros/transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"

void poseCallback(const turtlesim::Pose::ConstPtr& msg)
{
    // get pose and translate to relate 
    // 创建发布对象
    static tf2_ros::TransformBroadcaster pub; //静态的每次调用回调函数都是用同一个pub

    // 组织发布数据
    geometry_msgs::TransformStamped ts;
    ts.header.stamp = ros::Time::now();
    ts.header.frame_id = "world";
    ts.child_frame_id = "turtle1";
    ts.transform.translation.x = msg->x;
    ts.transform.translation.y = msg->y;
    ts.transform.translation.z = 0.0;

    tf2::Quaternion q;
    q.setRPY(0, 0, msg->theta);
    ts.transform.rotation.x = q.getX();
    ts.transform.rotation.y = q.getY();
    ts.transform.rotation.z = q.getZ(); // 位姿信息中没有四元素，但是有个偏航角度，又已知乌龟是2D，没有翻滚和俯仰角度，所以可以得出乌龟的四元素
    ts.transform.rotation.w = q.getW();

    pub.sendTransform(ts);

    // 发布数据
}
int main(int argc, char **argv)
{
    setlocale(LC_ALL, "");
    ros::init(argc, argv, "talker");
    // 发布方：需要订阅乌龟位姿信息，转换成相对于窗体的坐标关系，并发布准备
    // 准备：
    ros::NodeHandle n;
    ros::Subscriber sub = n.subscribe("turtle1/pose", 10, poseCallback);
    ros::spin();
    
}